#include <task.h>
#include <spinlock.h>
#include <log.h>
#include <core/sched.h>
#include <core/timer.h>
#include <switch_to.h>

LIST_HEAD(ready);
static spinlock_t sched_lock;

static inline struct thread_t *scheduler_next_ready_task()
{
    if (list_empty(&ready))
    {
        return NULL;
    }

    thread_t *thread = list_first_entry(&ready, thread_t, ready_queue_node);
    return thread;
}

void switch_mm(thread_t *thread)
{
    pagetable_active(thread->vmspace->pgtbl);
}

void scheduler_enqueue_task(thread_t *thread)
{
    // LOGE("thread:" $(thread));
    list_add_tail(&thread->ready_queue_node, &ready);
}

void scheduler_dequeue_task(thread_t *thread)
{
    list_del_init(&thread->ready_queue_node);
}

void schedule_to(thread_t *thread)
{
    spin_lock(&sched_lock);

    thread_t *current = thread_self();
    current->status = TASK_STATUS_SUSPEND;
    scheduler_dequeue_task(current);

    thread_t *old = current;
    current = thread_self_set(thread);
    current->status = TASK_STATUS_RUNNING;

    spin_unlock(&sched_lock);

    switch_mm(current);
    switch_to(old, current, old);
}

void schedule()
{
    // LOGI("");
    thread_t *current = thread_self();

    if ((!(current->thread_info.flags & NEED_RESCHED)) && current->status == TASK_STATUS_RUNNING)
    {
        // LOGI("continue to run");
        return;
    }

    irq_flags_t flags;
    spin_lock_irqsave(&sched_lock, flags);

    thread_clear_sched_flag(current);

    thread_t *thread = scheduler_next_ready_task();
    if (thread == NULL)
    {
        // LOGI("switch to idle");
        if (current->status != TASK_STATUS_RUNNING)
            thread = get_idle_thread();
        else
            thread = current;
    }

    // LOGD($(current) "[" $(current->name) "] ==> " $(thread) "[" $(thread->name) "]");

    if (current == thread)
    {
        scheduler_dequeue_task(current);
        spin_unlock_irqrestore(&sched_lock, flags);
        current->status = TASK_STATUS_RUNNING;
        return;
    }
    scheduler_dequeue_task(thread);

    if (current->status == TASK_STATUS_SUSPEND || current->status == TASK_STATUS_EXIT)
    {
        scheduler_dequeue_task(current);
    }
    else if (current != get_idle_thread())
    {
        scheduler_enqueue_task(current);
    }

    thread_t *old = current;

    current = thread_self_set(thread);

    current->status = TASK_STATUS_RUNNING;

    switch_mm(current);
    switch_to(old, current, old); // old is previous task afterswitch

    if (old->status == TASK_STATUS_EXIT)
    {
        delete (old);
    }
    // TODO: missing unlock if lanuch new thread
    spin_unlock_irqrestore(&sched_lock, flags);
}

static int sched_timer_function(struct timer_t *timer, void *data)
{
    thread_need_sched(thread_self());
    timer_forward_now(timer, ms_to_ktime(20));
    return 1;
}

void do_sched_init()
{
    spin_lock_init(&sched_lock);
    timer_t *timer = new (timer_t);
    timer_init(timer, sched_timer_function, NULL);
    timer_start_now(timer, ms_to_ktime(20));
}